/*
    六轴imu数据, 以加速度来的第一帧确定初始的翻滚角和俯仰角
    航向角初值为0; 起始时imu静止, 刚好符合初值=0
*/
#include "04imu2euler.hpp"

void callbackImu(const sensor_msgs::ImuConstPtr &imu_msg)
{   
    
    double t = imu_msg->header.stamp.toSec();
    double ax = imu_msg->linear_acceleration.x;
    double ay = imu_msg->linear_acceleration.y;
    double az = imu_msg->linear_acceleration.z;
    double gx = imu_msg->angular_velocity.x;
    double gy = imu_msg->angular_velocity.y;
    double gz = imu_msg->angular_velocity.z;
    
    Eigen::Matrix<double, 7, 1> imu_data;
    imu_data << t, ax, ay, az, gx, gy, gz;
    imu_lock.lock();
    imuQue.emplace(imu_data);
    imu_lock.unlock();
}

void initEuler()
{   
    bool first_data = true;
    double last_t, cur_t;
    Eigen::Vector3d last_acc, cur_acc, last_gyr, cur_gyr; 
    Eigen::Vector3d last_acc_euler, last_gyr_euler, cur_acc_euler, cur_gyr_euler;
    while (true)
    {
        if(imuQue.empty())
            continue;
        imu_lock.lock();
        Eigen::Matrix<double, 7, 1> imu_data = imuQue.front();
        imuQue.pop();
        imu_lock.unlock();

        cur_t = imu_data(0);
        cur_acc << imu_data(1), imu_data(2), imu_data(3);
        cur_gyr << imu_data(4), imu_data(5), imu_data(6);
        
        cur_acc.normalize();
        // roll前面没有
        cur_acc_euler.x() = atan2(cur_acc.y(), cur_acc.z());
        cur_acc_euler.y() = -atan2(cur_acc.x(), sqrt(cur_acc.y() * cur_acc.y() + cur_acc.z() * cur_acc.z())); // 正负号根据imu的四元素确定
        cur_acc_euler.z() = 0.;

        if(first_data)
        {
            last_t = cur_t;
            last_acc = cur_acc;
            last_gyr = cur_gyr;

            // 求第一帧的角速度算角度的角度初始值
            cur_gyr_euler = cur_acc_euler;   // 把加速度第一帧的投影作为角速度积分得初值
        }

        double dt = cur_t - last_t;

        if(dt > 0.0001)   // 不为0, 保证不是第一帧
        {   
            // 中值积分 也可以四元素积分(欧拉法) 中值积分精度高一点
            cur_gyr_euler = last_gyr_euler + 0.5 * (last_gyr + cur_gyr) * dt;
            
            // 限制角度在(-π, π)
            cur_gyr_euler(0) = atan2(sin(cur_gyr_euler(0)), cos(cur_gyr_euler(0)));
            cur_gyr_euler(1) = atan2(sin(cur_gyr_euler(1)), cos(cur_gyr_euler(1)));
            cur_gyr_euler(2) = atan2(sin(cur_gyr_euler(2)), cos(cur_gyr_euler(2)));
        
        }

        // 更新
        first_data = false;
        last_t = cur_t;
        last_acc = cur_acc;
        last_gyr = cur_gyr;
        last_gyr_euler = cur_gyr_euler;  

        // std::cout << (cur_acc_euler * 180. / M_PI).transpose() << std::endl;
        // std::cout << (cur_gyr_euler * 180. / M_PI).transpose() << std::endl;
        // std::cout << "---------\n" << std::endl;

        // 给融合使用fusionQue
        Eigen::Vector4d a_euler(cur_t, cur_acc_euler(0),cur_acc_euler(1), cur_acc_euler(2));
        Eigen::Vector4d g_euler(cur_t, cur_gyr_euler(0),cur_gyr_euler(1), cur_gyr_euler(2));
        
        fusion_lock.lock();
        fusionQue.emplace(a_euler, g_euler);
        fusion_lock.unlock();
    }
}

void fusionProcess()
{
    while (1)
    {
        if(fusionQue.empty())
            continue;
        fusion_lock.lock();
        Eigen::Vector3d euler_acc = fusionQue.front().first.block<3, 1>(1, 0);
        Eigen::Vector3d euler_gyr = fusionQue.front().second.block<3, 1>(1, 0);
        double t = fusionQue.front().first.block<1,1>(0, 0)(0);
        fusionQue.pop();
        fusion_lock.unlock();

       

        
        std::cout << std::fixed << std::setprecision(2) << (euler_acc * 180. / M_PI).transpose() << std::endl;
        std::cout << std::fixed << std::setprecision(2) << (euler_gyr * 180. / M_PI).transpose() << std::endl;
        
        std::cout << t << std::endl;
        std::cout << euler_acc << std::endl;
        std::cout << "---------\n" << std::endl;
    }
    
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "euler_node");
    ros::NodeHandle n;
    ros::Subscriber sub_imu = n.subscribe("/imu/data", 1000, &callbackImu);

    std::thread t1(initEuler);
    std::thread t2(fusionProcess);
    ros::spin();
    return 0;
}